In recent years, in order to improve safety to collision and to reduce loss due to destruction in a robot, high accuracy of collision detection is required. However, accurate collision sensor increases cost, and the collision sensor as a heavy load is swung, thereby being contrary to speeding up of the robot and energy saving. Therefore, it is desired to obtain a collision force accurately without the collision sensor.
A method for obtaining a collision force at the collision without using a collision sensor is a dynamic calculation system (see NPL 1). In the dynamic calculation system, a torque output from a gear reducer is obtained by subtracting a torque which is lost due to an inertia and friction between a motor and a gear reducer from a torque generated by a driving current of the motor. A collision force is obtained by subtracting the dynamic torque of the robot obtained from an inverse dynamic calculation of the robot using the torque output from the gear reducer.
FIG. 1 is a block diagram illustrating a conventional position control of a vertically articulated robot and illustrates an outline of internal configurations of robot mechanism 161 and robot control device 162. Robot mechanism 161 includes plural arms and joint axes. The arms and the joint axes of a robot are driven by gear reducers provided around the joint axes.
In FIG. 1, main controller 64 provided in robot control device 162 outputs position commands θcom (θcom1 to θcom6) of respective axes (for example, six axes) of the robot following a locus of a movement of an arm of robot mechanism 161 instructed and stored by operation/teaching unit 63. Servo controllers 165 (1651 to 1656) of respective axes of the robot control motors 66 (661 to 666) within robot mechanism 161 so as to follow position commands θcom (θcom1 to θcom6), respectively.
FIG. 2 is a block diagram of servo controller 165 shown in FIG. 1 in detail and adds a sensorless collision detection function without using the collision sensor of the dynamic calculation system.
In FIG. 2, position control block 6 generates speed loop command ωcom according to a difference value between position command θcom and motor position feedback θm. Motor position feedback θm is obtained from encoder 51 that is a position detector attached to motor 66.
Speed control block 10 shown in FIG. 2 generates motor current command Im according to a difference value between speed loop command ωcom and motor speed feedback ωm obtained by differentiating motor position feedback θm.
Block 18 shown in FIG. 2 represents motor 66 and an external force. Assuming that gear reducer 53 is rigid, motor-generated torque τm is expressed by Formula 1 with motor torque constant Kt, motor current Im, motor angular acceleration αm, motor angular velocity ωm, motor inertia Jm, viscous friction coefficient D, dynamic friction torque τμ dynamic torque τdyn, and collision torque τdis.τm=Kt×Im τm=τdyn+τdis+Jm×αm+D×Ωm+τμ  [Formula 1]
Motor angular acceleration αm is a differential value of motor angular velocity ωm. Motor inertia Jm is the sum of a rotor of motor 66 and a primary side of gear reducer 53. Dynamic torque τdyn is the sum of a gravity torque, an inertial force, a centrifugal force, and a Coriolis force.
Specifically, motor-generated torque τm is expressed by the first formula of Formula 1 viewing from a driving side of motor 66, and is expressed by the second formula of Formula 1 viewing from a load side via gear reducer 53 with respect to motor 66.
Dynamic friction torque τμ can be calculated by Formula 2 with dynamic friction coefficient Kμ.
                              τμ          =                      K            ⁢                                                  ⁢            μ            ×            sgn                          ⁢                                  ⁢                  sgn          =                      {                                                            1                                                                      (                                                                  ω                        ⁢                                                                                                  ⁢                        m                                            >                      0                                        )                                                                                                0                                                                      (                                                                  ω                        ⁢                                                                                                  ⁢                        m                                            =                      0                                        )                                                                                                                    -                    1                                                                                        (                                                                  ω                        ⁢                                                                                                  ⁢                        m                                            <                      0                                        )                                                                                                          [                  Formula          ⁢                                          ⁢          2                ]            
Collision torque τdis that is the right side of the second formula of Formula 1 can be obtained by transforming Formula 1 to the following Formula 3.τdis=(Kt×Im+Jm×αm−D×ωm−Kμ×sgn)−τdyn  [Formula 3]
In Formula 3, the term, (Kt×Im+Jm×αm−D×Ωm−Kμ×sgn), represents a torque output to gear reducer 53 by motor 66.
In FIG. 2, Formula 3 is expressed as torque estimation block 30.
In torque estimation block 30, dynamic torque estimation value τdyno is obtained by executing the inverse dynamic calculation by using the motor speed feedback and a mechanical parameter of the robot of all axes constituting the robot in torque calculation block 26. Torque estimation block 30 obtains collision torque estimation value τdiso using dynamic torque estimation value τdyno, and outputs collision torque estimation value τdiso to collision determination block 31.
Collision determination block 31 detects the collision according to Formula 4 with predetermined collision detection threshold τth.|τdiso|>τth  [Formula 4]
That is, in a case where Formula 4 is satisfied, main controller 64 determines that a collision occurs. In a case where Formula 4 is not satisfied, main controller 64 determines that a collision does not occur.
In torque estimation block 30 shown in FIG. 2 for obtaining collision torque estimation value τdiso, even in a case where the collision does not occur, collision torque estimation value τdiso does not become zero due to a parameter error, a model error of dynamic torque calculation, or the like. That is, in a case where the collision does not actually occur, an estimation error of the collision torque is output as collision torque estimation value τdiso.
In particular, an error of viscous friction coefficient D among the parameter errors is increased at a low temperature of about 5° C. due to an increase of viscosity of grease. The error is increased if the collision torque estimation value τdiso is obtained by using viscous friction coefficient D measured at a normal temperature. Therefore, collision detection threshold τth may be increased in order to prevent erroneous collision detection.
PTL 1 discloses a method of measuring a temperature and raising the collision detection threshold at a low temperature is disclosed, as described below.
FIGS. 3 and 4 illustrate other conventional techniques corresponding to FIGS. 1 and 2, respectively. A temperature sensor is attached to a gear reducer, and a configuration for switching the collision detection threshold is added.
In FIG. 3, temperature sensor 56 is attached to gear reducer 53. Temperature sensor output value Tc of temperature sensor 56 is input into first servo controller 165.
In FIG. 4, in threshold switching block 34, temperature sensor output value Tc is compared to predetermined temperature threshold Tcth, and collision detection threshold τvth is output to collision determination block 40 according to Formula 5 with collision detection threshold τth at a normal temperature and threshold increment dτth corresponding to a low temperature (dτth>0).
                              τ          ⁢                                          ⁢          vth                =                  {                                                                      τ                  ⁢                                                                          ⁢                  th                                                                              (                                      Tc                    ≥                    Tcth                                    )                                                                                                                          τ                    ⁢                                                                                  ⁢                    th                                    +                                      d                    ⁢                                                                                  ⁢                    τ                    ⁢                                                                                  ⁢                    th                                                                                                (                                      Tc                    <                    Tcth                                    )                                                                                        [                  Formula          ⁢                                          ⁢          5                ]            
Collision detection threshold τth at a normal temperature is previously obtained by actually operating gear reducer 53 at a normal temperature, for example, equal to or higher than 5° C. Threshold increment dτth corresponding to a low temperature is previously obtained by actually operating gear reducer 53 at a low temperature. for example, lower than 5° C.
Collision determination block 40 detects the collision according to Formula 6 with collision detection threshold τvth calculated in Formula 5.|τdiso|>τvth  [Formula 6]
That is, in a case where Formula 6 is satisfied, main controller 64 determines that a collision occurs. In a case where Formula 6 is not satisfied, main controller 64 determines that a collision does not occur.